// UC1609C 192*64 LCM 的 AVR 驱动。
// 本驱动需要根据接线修改端口设置，设置在configure.h
// 源代码发布在  https://gitee.com/redpower/uc1609c_avr
// 木兰开放许可协议
//  
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <util/delay.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "lcd_uc1609c.h"
#include "lcd_fonts.h"
#include "configure.h"

uchar screen_buffer[BUF_WIDTH * BUF_HEIGHT / 8];

uchar PROGMEM DarkScreen[] = {0x00, 0x00, 0x00, 0x00};
uchar PROGMEM BrightScreen[] = {0xff, 0xff, 0xff, 0xff};

//PROGMEM
uchar PROGMEM IC[] = {

    0xe2, // system reset

    0xa0, // set Frame Rate[A0: 76fps, A1b: 95fps, A2b: 132fps, A3b: 168fps(fps:
          // frame-per-second)]

    0xeb, // set BR
    0x2f, // set Power Control

    0xc2, // set LCD Mapping Control
    0x81, // set PM
    180,  // set PM 110
    0xaf, // display Enable
};


void lcd_write_raw(uchar data1)
{
  uchar i, temp = data1;
  for (i = 0; i < 8; i++)
  {

    if ((temp & 0x80) == 0x80) // SDA=(bit)(temp&0x80);
      SET_SDA_1;               //PORTA |= (1 << SDA);
    else
      SET_SDA_0; //PORTA &= ~(1 << SDA);

    SET_SCK_0; //PORTA &= ~(1 << SCK); // SCK=0;
    SET_SCK_1; //PORTA |= (1 << SCK); // SCK=1;
    temp <<= 1;
  }
}

void lcd_write_cmd(uchar command)
{
  SET_CS_0;
  SET_CD_0;
  lcd_write_raw(command);
  SET_CD_1;
  SET_CS_1;
}
void lcd_write_data(uchar data1)
{
  SET_CS_0;
  SET_CD_1;
  lcd_write_raw(data1);
  SET_CD_1;
  SET_CS_1;
}
void lcd_write_cmd_ex(uint8_t command, uint8_t value)
{
  SET_CS_0;
  SET_CD_0;
  lcd_write_raw(command | value);
  SET_CD_1;
  SET_CS_1;
}

//LCD 初始化
void lcd_init(void)
{
  int i;
  SET_CS_1;
  SET_RST_0;    //PORTA &= ~(1 << RST); // RST=0;
  _delay_ms(3); // 3
  SET_RST_1;    //PORTA |= (1 << RST);  // RST=1;
  _delay_ms(6); // 6

  for (i = 0; i < 8; i++)
  {
    lcd_write_cmd(pgm_read_byte(&IC[i]));
  }
  graphics_clear_buf();
}
//使用某种模式填充屏幕，
//默认array可以用DarkScreen 和 BrightScreen ，也可以自定义
void lcd_format_screen(uchar array[])
{
  uint i, j, k;
  for (i = 0; i < 9; i++)
  {
    lcd_write_cmd(0x40);     // set scroll line
    lcd_write_cmd(0xb0 | i); // set page address
    lcd_write_cmd(0x10);     // column  msb
    lcd_write_cmd(0x00);     // column  lsb
    for (j = 0; j < 48; j++)
      for (k = 0; k < 4; k++)
      {
        lcd_write_data(pgm_read_byte(&array[k]));
      }
  }
}
//设置暗背景
void lcd_set_dark()
{
	lcd_format_screen(DarkScreen);
}
//设置亮背景
void lcd_set_bright()
{
	lcd_format_screen(BrightScreen);
}

void lcd_address(uint page, uint column)
{
  column = column - 0x01;
  lcd_write_cmd(0xB0 + page - 1);             //设置页地址，每8 行为一页，全屏共64 行，被分成8 页
  lcd_write_cmd(0x10 + (column >> 4 & 0x0F)); //设置列地址的高4 位
  lcd_write_cmd(column & 0x0F);               //设置列地址的低4 位
}

//显示16x16 点阵图像、汉字、生僻字或16x16 点阵的其他图标
void lcd_display_16x16(uchar page, uchar column, uchar *dp)
{
  uint i, j;
  uint count = 0;
  for (j = 0; j < 2; j++)
  {
    lcd_address(page + j, column);
    for (i = 0; i < 16; i++)
    {
      lcd_write_data(pgm_read_byte(dp + count)); //写数据到LCD,每写完一个8 位的数据后列地址自动加1
      count++;
    }
  }
}


//显示32x64 点阵图像、汉字、生僻字或32x64 点阵的其他图标
void lcd_display_32x64(uchar page, uchar column, uchar *dp)
{
  uint i, j;
  uint count = 0;
  for (j = 0; j < 8; j++)
  {
    lcd_address(page + j, column);
    for (i = 0; i < 32; i++)
    {
      lcd_write_data(pgm_read_byte(dp + count)); //写数据到LCD,每写完一个8 位的数据后列地址自动加1
      count++;
    }
  }
}


// Desc: draws passed character.
// Param1: character 'A' or number in  the ASCII table 1-127(default)
void lcd_put_char_7x5(uchar page, uchar column, char character)
{
  lcd_address(page, column);
  for (uint8_t column = 0; column < 5; column++) //FONTWIDTH = 5
  {
    lcd_write_data((pgm_read_byte(custom_font + (character * 5) + column)));
  }
}

//指定坐标显示一个6*8字符串
void lcd_put_chars_7x5(uchar page, uchar column, char *data_s)
{
  while (*data_s)
  {
    lcd_put_char_7x5(page, column, *data_s); //显示单个字符
    data_s++;
    column += 6;

    if (column >= 186)
    {
      column = 1;
      page++;
      if (page > 8)
        return;
    }
  }
}

//显示8*16点阵ASCII字符函数
void lcd_put_char_16x8(uchar page, uchar column, char char_data)
{
  uint i, j;
  uint count = 0;
  for (j = 0; j < 2; j++)
  {
    lcd_address(page + j, column);
    //写入16字节点阵数字
    for (i = 0; i < 8; i++)
    {
      lcd_write_data(pgm_read_byte(&Font816[char_data - 0x20][count])); //写数据到LCD,每写完一个8 位的数据后列地址自动加1
      count++;
    }
  }
}
//指定坐标显示一个16*8字符串
void lcd_put_chars_16x8(uchar page, uchar column, char *data_s)
{
  while (*data_s)
  {
    lcd_put_char_16x8(page, column, *data_s); //显示单个字符
    data_s++;
    column += 8;

    if (column >= 184)
    {
      column = 1;
      page += 2;
      if (page > 8)
        return;
    }
  }
}

// Desc: invert the display
// Param1: bits, 1 invert , 0 normal
void lcd_invert_display(uchar is_invert)
{
  lcd_write_cmd_ex(UC1609_INVERSE_DISPLAY, is_invert); //#define UC1609_INVERSE_DISPLAY 0xA6 // inverts display
}


// Desc: turns in display
// Param1: bits 1  on , 0 off
void lcd_enable(uint8_t bits)
{
  lcd_write_cmd_ex(UC1609_DISPLAY_ON, bits);
}

// Desc: Scroll the displayed image up by SL rows.
//The valid SL value is between 0 (for no
//scrolling) and (64).
//Setting SL outside of this range causes undefined effect on the displayed
//image.
// Param1: bits 0-64 line number y-axis
void lcd_scroll(uint8_t bits)
{
  lcd_write_cmd_ex(UC1609_SCROLL, bits);
}

// Desc: Rotates the display
// Set LC[2:1] for COM (row) mirror (MY), SEG (column) mirror (MX).
// Param1: 4 possible values 000 010 100 110 (defined)
// If Mx is changed the buffer must BE updated
void lcd_rotate(uint8_t rotatevalue)
{
  switch (rotatevalue)
  {
  case 0:
    rotatevalue = 0;
    break;
  case 0x02:
    rotatevalue = UC1609_ROTATION_FLIP_ONE;
    break;
  case 0x04:
    rotatevalue = UC1609_ROTATION_NORMAL;
    break;
  case 0x06:
    rotatevalue = UC1609_ROTATION_FLIP_TWO;
    break;
  default:
    rotatevalue = UC1609_ROTATION_NORMAL;
    break;
  }
  lcd_write_cmd_ex(UC1609_LCD_CONTROL, rotatevalue);
}

// Desc: turns on all Pixels
// Param1: bits Set DC[1] to force all SEG drivers to output ON signals
// 1 all on ,  0 all off
void lcd_all_pixels_on(uint8_t bits)
{
  lcd_write_cmd_ex(UC1609_ALL_PIXEL_ON, bits);
}

// Desc: Fill the screen NOT the buffer with a datapattern
// Param1: datapattern can be set to zero to clear screen (not buffer) range 0x00 to 0ff
// Param2: optional delay in microseconds can be set to zero normally.
void lcd_fill_screen(uint8_t dataPattern)
{
  uint16_t numofbytes = LCD_WIDTH * (LCD_HEIGHT / 8); // width * height
  for (uint16_t i = 0; i < numofbytes; i++)
  {
    lcd_write_data(dataPattern);
  }
}

//Desc: Draw a bitmap to the screen
//Param1: x offset 0-192
//Param2: y offset 0-64
//Param3: width 0-192
//Param4 height 0-64
//Param5 the bitmap
void lcd_draw_bitmap_p(int16_t x, int16_t y, uint8_t w, uint8_t h, uint8_t *data) //使用程序存储器
{
  uint8_t tx, ty;
  uint16_t offset = 0;
  uint8_t column = (x < 0) ? 0 : x;
  uint8_t page = (y < 0) ? 0 : y / 8;

  for (ty = 0; ty < h; ty = ty + 8)
  {
    if (y + ty < 0 || y + ty >= LCD_HEIGHT)
    {
      continue;
    }

    lcd_write_cmd_ex(UC1609_SET_COLADD_LSB, (column & 0x0F));
    lcd_write_cmd_ex(UC1609_SET_COLADD_MSB, (column & 0XF0) >> 4);
    lcd_write_cmd_ex(UC1609_SET_PAGEADD, page++);

    for (tx = 0; tx < w; tx++)
    {
      if (x + tx < 0 || x + tx >= LCD_WIDTH)
      {
        continue;
      }
      offset = (w * (ty >> 3)) + tx;
      lcd_write_data(pgm_read_byte(&data[offset]));
    }
  }
}
//Desc: Draw a bitmap to the screen
//Param1: x offset 0-192
//Param2: y offset 0-64
//Param3: width 0-192
//Param4 height 0-64
//Param5 the bitmap
void lcd_draw_bitmap_mem(int16_t x, int16_t y, uint8_t w, uint8_t h, uint8_t *data) //使用内存
{
  uint8_t tx, ty;
  uint16_t offset = 0;
  uint8_t column = (x < 0) ? 0 : x;
  uint8_t page = (y < 0) ? 0 : y / 8;

  for (ty = 0; ty < h; ty = ty + 8)
  {
    if (y + ty < 0 || y + ty >= LCD_HEIGHT)
    {
      continue;
    }

    lcd_write_cmd_ex(UC1609_SET_COLADD_LSB, (column & 0x0F));
    lcd_write_cmd_ex(UC1609_SET_COLADD_MSB, (column & 0XF0) >> 4);
    lcd_write_cmd_ex(UC1609_SET_PAGEADD, page++);

    for (tx = 0; tx < w; tx++)
    {
      if (x + tx < 0 || x + tx >= LCD_WIDTH)
      {
        continue;
      }
      offset = (w * (ty >> 3)) + tx;
      lcd_write_data(data[offset]);
    }
  }
}


//清除图形显示缓冲区
void graphics_clear_buf() 
{
  memset(screen_buffer, 0, BUF_WIDTH * BUF_HEIGHT / 8);
}

//将缓冲区数据刷新到lcd
void graphics_update()
{
	lcd_draw_bitmap_mem(0, 0, BUF_WIDTH, BUF_HEIGHT, screen_buffer);
}

void graphics_draw_pixel(int16_t x, int16_t y, uint16_t colour)
{
  if ((x < 0) || (x >= BUF_WIDTH) || (y < 0) || (y >= BUF_HEIGHT))
  {
    return;
  }
  uint16_t tc = (BUF_WIDTH * (y / 8)) + x;
  switch (colour)
  {
  case FOREGROUND:
    screen_buffer[tc] |= (1 << (y & 7));
    break;
  case BACKGROUND:
    screen_buffer[tc] &= ~(1 << (y & 7));
    break;
  case INVERSE:
    screen_buffer[tc] ^= (1 << (y & 7));
    break;
  }
}



// Draw a circle outline
void graphics_draw_circle(int16_t x0, int16_t y0, int16_t r,
                uint16_t color)
{
  int16_t f = 1 - r;
  int16_t ddF_x = 1;
  int16_t ddF_y = -2 * r;
  int16_t x = 0;
  int16_t y = r;

  graphics_draw_pixel(x0, y0 + r, color);
  graphics_draw_pixel(x0, y0 - r, color);
  graphics_draw_pixel(x0 + r, y0, color);
  graphics_draw_pixel(x0 - r, y0, color);

  while (x < y)
  {
    if (f >= 0)
    {
      y--;
      ddF_y += 2;
      f += ddF_y;
    }
    x++;
    ddF_x += 2;
    f += ddF_x;

    graphics_draw_pixel(x0 + x, y0 + y, color);
    graphics_draw_pixel(x0 - x, y0 + y, color);
    graphics_draw_pixel(x0 + x, y0 - y, color);
    graphics_draw_pixel(x0 - x, y0 - y, color);
    graphics_draw_pixel(x0 + y, y0 + x, color);
    graphics_draw_pixel(x0 - y, y0 + x, color);
    graphics_draw_pixel(x0 + y, y0 - x, color);
    graphics_draw_pixel(x0 - y, y0 - x, color);
  }
}

void graphics_draw_circle_helper(int16_t x0, int16_t y0,
                      int16_t r, uint8_t cornername, uint16_t color)
{
  int16_t f = 1 - r;
  int16_t ddF_x = 1;
  int16_t ddF_y = -2 * r;
  int16_t x = 0;
  int16_t y = r;

  while (x < y)
  {
    if (f >= 0)
    {
      y--;
      ddF_y += 2;
      f += ddF_y;
    }
    x++;
    ddF_x += 2;
    f += ddF_x;
    if (cornername & 0x4)
    {
      graphics_draw_pixel(x0 + x, y0 + y, color);
      graphics_draw_pixel(x0 + y, y0 + x, color);
    }
    if (cornername & 0x2)
    {
      graphics_draw_pixel(x0 + x, y0 - y, color);
      graphics_draw_pixel(x0 + y, y0 - x, color);
    }
    if (cornername & 0x8)
    {
      graphics_draw_pixel(x0 - y, y0 + x, color);
      graphics_draw_pixel(x0 - x, y0 + y, color);
    }
    if (cornername & 0x1)
    {
      graphics_draw_pixel(x0 - y, y0 - x, color);
      graphics_draw_pixel(x0 - x, y0 - y, color);
    }
  }
}
void graphics_fill_circle(int16_t x0, int16_t y0, int16_t r,
                uint16_t color)
{
  graphics_draw_fast_vline(x0, y0 - r, 2 * r + 1, color);
  graphics_fill_circle_helper(x0, y0, r, 3, 0, color);
}

// Used to do circles and roundrects
void graphics_fill_circle_helper(int16_t x0, int16_t y0, int16_t r,
                      uint8_t cornername, int16_t delta, uint16_t color)
{

  int16_t f = 1 - r;
  int16_t ddF_x = 1;
  int16_t ddF_y = -2 * r;
  int16_t x = 0;
  int16_t y = r;

  while (x < y)
  {
    if (f >= 0)
    {
      y--;
      ddF_y += 2;
      f += ddF_y;
    }
    x++;
    ddF_x += 2;
    f += ddF_x;

    if (cornername & 0x1)
    {
      graphics_draw_fast_vline(x0 + x, y0 - y, 2 * y + 1 + delta, color);
      graphics_draw_fast_vline(x0 + y, y0 - x, 2 * x + 1 + delta, color);
    }
    if (cornername & 0x2)
    {
      graphics_draw_fast_vline(x0 - x, y0 - y, 2 * y + 1 + delta, color);
      graphics_draw_fast_vline(x0 - y, y0 - x, 2 * x + 1 + delta, color);
    }
  }
}
// Bresenham's algorithm - thx wikpedia
void graphics_draw_line(int16_t x0, int16_t y0,
              int16_t x1, int16_t y1,
              uint16_t color)
{
  int16_t steep = abs(y1 - y0) > abs(x1 - x0);
  if (steep)
  {
    swap(x0, y0);
    swap(x1, y1);
  }

  if (x0 > x1)
  {
    swap(x0, x1);
    swap(y0, y1);
  }

  int16_t dx, dy;
  dx = x1 - x0;
  dy = abs(y1 - y0);

  int16_t err = dx / 2;
  int16_t ystep;

  if (y0 < y1)
  {
    ystep = 1;
  }
  else
  {
    ystep = -1;
  }

  for (; x0 <= x1; x0++)
  {
    if (steep)
    {
      graphics_draw_pixel(y0, x0, color);
    }
    else
    {
      graphics_draw_pixel(x0, y0, color);
    }
    err -= dy;
    if (err < 0)
    {
      y0 += ystep;
      err += dx;
    }
  }
}

// Draw a rectangle
void graphics_draw_rect(int16_t x, int16_t y,
              int16_t w, int16_t h,
              uint16_t color)
{
  graphics_draw_fast_hline(x, y, w, color);
  graphics_draw_fast_hline(x, y + h - 1, w, color);
  graphics_draw_fast_vline(x, y, h, color);
  graphics_draw_fast_vline(x + w - 1, y, h, color);
}

void graphics_draw_fast_vline(int16_t x, int16_t y,
                   int16_t h, uint16_t color)
{
  // Update in subclasses if desired!
  graphics_draw_line(x, y, x, y + h - 1, color);
}

void graphics_draw_fast_hline(int16_t x, int16_t y,
                   int16_t w, uint16_t color)
{
  // Update in subclasses if desired!
  graphics_draw_line(x, y, x + w - 1, y, color);
}

void graphics_fill_rect(int16_t x, int16_t y, int16_t w, int16_t h,
              uint16_t color)
{
  // Update in subclasses if desired!
  for (int16_t i = x; i < x + w; i++)
  {
    graphics_draw_fast_vline(i, y, h, color);
  }
}

void graphics_fill_screen(uint16_t color)
{
  graphics_fill_rect(0, 0, BUF_WIDTH, BUF_HEIGHT, color);
}

// Draw a rounded rectangle
void graphics_draw_round_rect(int16_t x, int16_t y, int16_t w,
                   int16_t h, int16_t r, uint16_t color)
{
  // smarter version
  graphics_draw_fast_hline(x + r, y, w - 2 * r, color);         // Top
  graphics_draw_fast_hline(x + r, y + h - 1, w - 2 * r, color); // Bottom
  graphics_draw_fast_vline(x, y + r, h - 2 * r, color);         // Left
  graphics_draw_fast_vline(x + w - 1, y + r, h - 2 * r, color); // Right
  // draw four corners
  graphics_draw_circle_helper(x + r, y + r, r, 1, color);
  graphics_draw_circle_helper(x + w - r - 1, y + r, r, 2, color);
  graphics_draw_circle_helper(x + w - r - 1, y + h - r - 1, r, 4, color);
  graphics_draw_circle_helper(x + r, y + h - r - 1, r, 8, color);
}

// Fill a rounded rectangle
void graphics_fill_round_rect(int16_t x, int16_t y, int16_t w,
                   int16_t h, int16_t r, uint16_t color)
{
  // smarter version
  graphics_fill_rect(x + r, y, w - 2 * r, h, color);

  // draw four corners
  graphics_fill_circle_helper(x + w - r - 1, y + r, r, 1, h - 2 * r - 1, color);
  graphics_fill_circle_helper(x + r, y + r, r, 2, h - 2 * r - 1, color);
}

// Draw a triangle
void graphics_draw_triangle(int16_t x0, int16_t y0,
                  int16_t x1, int16_t y1,
                  int16_t x2, int16_t y2, uint16_t color)
{
  graphics_draw_line(x0, y0, x1, y1, color);
  graphics_draw_line(x1, y1, x2, y2, color);
  graphics_draw_line(x2, y2, x0, y0, color);
}

// Fill a triangle
void graphics_fill_triangle(int16_t x0, int16_t y0,
                  int16_t x1, int16_t y1,
                  int16_t x2, int16_t y2, uint16_t color)
{

  int16_t a, b, y, last;

  // Sort coordinates by Y order (y2 >= y1 >= y0)
  if (y0 > y1)
  {
    swap(y0, y1);
    swap(x0, x1);
  }
  if (y1 > y2)
  {
    swap(y2, y1);
    swap(x2, x1);
  }
  if (y0 > y1)
  {
    swap(y0, y1);
    swap(x0, x1);
  }

  if (y0 == y2)
  { // Handle awkward all-on-same-line case as its own thing
    a = b = x0;
    if (x1 < a)
      a = x1;
    else if (x1 > b)
      b = x1;
    if (x2 < a)
      a = x2;
    else if (x2 > b)
      b = x2;
    graphics_draw_fast_hline(a, y0, b - a + 1, color);
    return;
  }

  int16_t
      dx01 = x1 - x0,
      dy01 = y1 - y0,
      dx02 = x2 - x0,
      dy02 = y2 - y0,
      dx12 = x2 - x1,
      dy12 = y2 - y1;
  int32_t
      sa = 0,
      sb = 0;

  // For upper part of triangle, find scanline crossings for segments
  // 0-1 and 0-2.  If y1=y2 (flat-bottomed triangle), the scanline y1
  // is included here (and second loop will be skipped, avoiding a /0
  // error there), otherwise scanline y1 is skipped here and handled
  // in the second loop...which also avoids a /0 error here if y0=y1
  // (flat-topped triangle).
  if (y1 == y2)
    last = y1; // Include y1 scanline
  else
    last = y1 - 1; // Skip it

  for (y = y0; y <= last; y++)
  {
    a = x0 + sa / dy01;
    b = x0 + sb / dy02;
    sa += dx01;
    sb += dx02;
    /* longhand:
    a = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
    b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
    */
    if (a > b)
      swap(a, b);
    graphics_draw_fast_hline(a, y, b - a + 1, color);
  }

  // For lower part of triangle, find scanline crossings for segments
  // 0-2 and 1-2.  This loop is skipped if y1=y2.
  sa = dx12 * (y - y1);
  sb = dx02 * (y - y0);
  for (; y <= y2; y++)
  {
    a = x1 + sa / dy12;
    b = x0 + sb / dy02;
    sa += dx12;
    sb += dx02;
    /* longhand:
    a = x1 + (x2 - x1) * (y - y1) / (y2 - y1);
    b = x0 + (x2 - x0) * (y - y0) / (y2 - y0);
    */
    if (a > b)
      swap(a, b);
    graphics_draw_fast_hline(a, y, b - a + 1, color);
  }
}

void graphics_draw_bitmap(int16_t x, int16_t y,
                const uint8_t *bitmap, int16_t w, int16_t h,
                uint16_t color)
{

  int16_t i, j, byteWidth = (w + 7) / 8;

  for (j = 0; j < h; j++)
  {
    for (i = 0; i < w; i++)
    {
      if (pgm_read_byte(bitmap + j * byteWidth + i / 8) & (128 >> (i & 7)))
      {
        graphics_draw_pixel(x + i, y + j, color);
      }
    }
  }
}

// Draw a 1-bit color bitmap at the specified x, y position from the
// provided bitmap buffer (must be PROGMEM memory) using color as the
// foreground color and bg as the background color.
void graphics_draw_bitmap_ex(int16_t x, int16_t y,
                   const uint8_t *bitmap, int16_t w, int16_t h,
                   uint16_t color, uint16_t bg)
{

  int16_t i, j, byteWidth = (w + 7) / 8;

  for (j = 0; j < h; j++)
  {
    for (i = 0; i < w; i++)
    {
      if (pgm_read_byte(bitmap + j * byteWidth + i / 8) & (128 >> (i & 7)))
      {
        graphics_draw_pixel(x + i, y + j, color);
      }
      else
      {
        graphics_draw_pixel(x + i, y + j, bg);
      }
    }
  }
}

//Draw XBitMap Files (*.xbm), exported from GIMP,
//Usage: Export from GIMP to *.xbm, rename *.xbm to *.c and open in editor.
//C Array can be directly used with this function
void graphics_draw_xbitmap(int16_t x, int16_t y,
                 const uint8_t *bitmap, int16_t w, int16_t h,
                 uint16_t color)
{

  int16_t i, j, byteWidth = (w + 7) / 8;

  for (j = 0; j < h; j++)
  {
    for (i = 0; i < w; i++)
    {
      if (pgm_read_byte(bitmap + j * byteWidth + i / 8) & (1 << (i % 8)))
      {
        graphics_draw_pixel(x + i, y + j, color);
      }
    }
  }
}

// Draw a character
void graphics_draw_char(int16_t x, int16_t y, unsigned char c,
              uint16_t color, uint16_t bg, uint8_t size)
{

  if ((x >= BUF_WIDTH) ||         // Clip right
      (y >= BUF_HEIGHT) ||        // Clip bottom
      ((x + 6 * size - 1) < 0) || // Clip left
      ((y + 8 * size - 1) < 0))   // Clip top
    return;

  for (int8_t i = 0; i < 6; i++)
  {
    uint8_t line;
    if (i == 5)
    {
      line = 0x0;
    }
    else
    {
      line = pgm_read_byte(custom_font + (c * 5) + i);
    }
    for (int8_t j = 0; j < 8; j++)
    {
      if (line & 0x1)
      {
        if (size == 1) // default size
          graphics_draw_pixel(x + i, y + j, color);
        else
        { // big size
          graphics_fill_rect(x + (i * size), y + (j * size), size, size, color);
        }
      }
      else if (bg != color)
      {
        if (size == 1) // default size
          graphics_draw_pixel(x + i, y + j, bg);
        else
        { // big size
          graphics_fill_rect(x + i * size, y + j * size, size, size, bg);
        }
      }
      line >>= 1;
    }
  }
}
